package pathfinding;

import java.io.Serializable;
import java.util.ArrayList;
import java.util.Collection;

import base.Timestamped;
import robotutils.planning.DStarLite;

public abstract class DStarGridDiag extends DStarLite<Timestamped<GridCell>> implements Serializable {
	private static final long serialVersionUID = 1L;

	public DStarGridDiag(Timestamped<GridCell> start, Timestamped<GridCell> goal) {
		super(start, goal);
	}

	protected abstract long timeToNext();
	
	protected abstract boolean isReserved(GridCell c, long timeStart, long timeEnd);
	
	protected static final double diagCost = Math.sqrt(2);
	
	protected Collection<Timestamped<GridCell>> nbrs(Timestamped<GridCell> s, boolean inFuture) {
		Collection<Timestamped<GridCell>> result = new ArrayList<Timestamped<GridCell>> (8);
		long timestamp = s.timestamp;
		if (inFuture) {
			timestamp += timeToNext();
		}
		else {
			timestamp -= timeToNext();
		}
			
		GridCell c = s.thing;
		
		result.add(new Timestamped<GridCell>(c.north()    , timestamp));
		result.add(new Timestamped<GridCell>(c.northeast(), timestamp));
		result.add(new Timestamped<GridCell>(c.east()     , timestamp));
		result.add(new Timestamped<GridCell>(c.southeast(), timestamp));
		result.add(new Timestamped<GridCell>(c.south()    , timestamp));
		result.add(new Timestamped<GridCell>(c.southwest(), timestamp));
		result.add(new Timestamped<GridCell>(c.west()     , timestamp));
		result.add(new Timestamped<GridCell>(c.northwest(), timestamp));
		
		return result;
	}
	
	protected Collection<Timestamped<GridCell>> freeNbrs(Timestamped<GridCell> s, boolean inFuture) {
		Collection<Timestamped<GridCell>> result = new ArrayList<Timestamped<GridCell>> (8); 
		long timeStart = s.timestamp, extraTime;
		
		extraTime = timeToNext()/2;
		if (!inFuture) {
			extraTime = -extraTime;
		}
		
		for (Timestamped<GridCell> c : nbrs(s, inFuture))
		{
			if (isReserved(c.thing, timeStart, c.timestamp + extraTime) == false)
			{
				result.add(c);
			}
		}
		return result;
	}
	
	@Override
	protected Collection<Timestamped<GridCell>> succ(Timestamped<GridCell> s) {
		return freeNbrs(s, true);
	}

	@Override
	protected Collection<Timestamped<GridCell>> pred(Timestamped<GridCell> s) {
		return freeNbrs(s, false);
	}

	@Override
	protected double h(Timestamped<GridCell> a, Timestamped<GridCell> b) {
		// TODO Auto-generated method stub
		return manhattanDist(a, b);
	}

	@Override
	protected double c(Timestamped<GridCell> a, Timestamped<GridCell> b) {
		double mdist = manhattanDist(a, b); 
		if (mdist == 1)
		{
			return 1;
		}
		else if (mdist == 2)
		{
			return diagCost;
		}
		else {
			throw new IllegalStateException();
		}
	}

	protected double manhattanDist(Timestamped<GridCell> a, Timestamped<GridCell> b)
	{
		return Math.abs(a.thing.x - b.thing.x) + Math.abs(a.thing.y - b.thing.y);
	}
}
